#include <driver/hardware_interface.h>
#include <realtime_tools/realtime_publisher.h>
#include <iostream>
#include <motor/motor_factory.h>
#include <encoder/encoder_factory.h>
#include <common/common.h>

namespace ros_canopen_driver {

HardwareInterface::HardwareInterface(rclcpp::Node& nh) :
		nh_(nh) {
	// Initialize shared memory and interfaces here
	init(); // this implementation loads from rosparam

	RCLCPP_INFO(rclcpp::get_logger("Driver"), "[HardwareInterface] Loaded hardware_interface.");
}

HardwareInterface::~HardwareInterface()
{
	quit();
}

void HardwareInterface::quit()
{
	for (uint8_t i = 0;i < servo_.size(); i++) {
		// TODO: add encoder operate for sddriver; add lift_monitor.stop();
		servo_[i]->Motor_Shutdown();
		delete servo_[i];
		servo_[i] = nullptr;
	}

	for (uint8_t i=0; i<encoder_.size(); i++) {
		delete encoder_[i];
		encoder_[i] = nullptr;
	}
}

void HardwareInterface::init() 
{
	ep_.init(nh_);

	RCLCPP_INFO(rclcpp::get_logger("Driver"), "[HardwareInterface] Reading rosparams from namespace: %s", nh_.getNamespace().c_str());

	joint_names_ = common::get_single_layer_param_names("/joint");

	if (joint_names_.size() == 0) {
		ROS_FATAL_STREAM_NAMED("hardware_interface",
				"No joints found on parameter server for controller, did you load the proper yaml file?" << " Namespace: " << nh_.getNamespace());
		exit(-1);
	}

	num_joints_ = joint_names_.size();

	// Resize vectors
	joint_position_.resize(num_joints_);
	joint_velocity_.resize(num_joints_);
	joint_effort_.resize(num_joints_);
	joint_timestamp_.resize(num_joints_);
	joint_position_command_.resize(num_joints_);
	joint_velocity_command_.resize(num_joints_);
	joint_effort_command_.resize(num_joints_);

	joint_position_.assign(num_joints_, 0.0);
	joint_velocity_.assign(num_joints_, 0.0);
	joint_effort_.assign(num_joints_, 0.0);
	joint_timestamp_.assign(num_joints_, 0.0);
	joint_position_command_.assign(num_joints_, 0.0);
	joint_velocity_command_.assign(num_joints_, 0.0);
	joint_effort_command_.assign(num_joints_, 0.0);


	servo_.resize(0);
	encoder_.resize(0);
	jhc_.clear();

	// diagnostics_pub_ = nh_.advertise<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 1);
	// servo_eventsMsg_.nodeId = ROS_NODEID;
	// servo_eventsMsg_.nodeName = ros::this_node::getName();
	// ROS_INFO_STREAM("get node name is: "<< servo_eventsMsg_.nodeName);
	// 2023.12.15 modify by haibo
    event_pub_ = nh_.advertise<agv_msgs::msg::AGVEvent>("/agvevent", 1);
    driversInfo_pub_ = nh_.advertise<agv_msgs::msg::DriversInfo>("driversInfo", 1);

    ROS_ASSERT(num_joints_ > 0);

	// Initialize controller
	for (std::size_t i = 0; i < num_joints_; ++i) {
		ROS_DEBUG_STREAM_NAMED("hardware_interface",
				"Loading joint name: " << joint_names_[i]);
		resource_manager::JointHandle* j = new resource_manager::JointHandle(joint_names_[i], &joint_position_[i], &joint_velocity_[i], &joint_effort_[i],
										&joint_timestamp_[i], &joint_position_command_[i], &joint_velocity_command_[i], &joint_effort_command_[i]);
		jhc_.registerHandle(j);
		ROS_INFO_STREAM("Loading joint name: " << joint_names_[i]);      		

		std::string temp;
		std::string s_name = joint_names_[i];
		int s_nodeid = -1;
		int s_manufacturer = 0;

		temp = "/joint/" + s_name + "/manufacturer";
		nh_.param(temp.c_str(), s_manufacturer, 0);
		temp = "/joint/" + s_name + "/nodeId";
		nh_.param(temp.c_str(), s_nodeid, -1);

		if (s_manufacturer < 20) {
			Motor *s = MotorFactory::generateMotor(nh_, i, s_name, s_manufacturer);
			s->nodeid = s_nodeid;
			servo_.push_back(s);
			j->registerEnableFunc(std::bind(&Motor::Motor_Recover, s));
			j->registerDisableFunc(std::bind(&Motor::Motor_Halt, s));
		}
		else {
			encoder::Encoder *e = encoder::EncoderFactory::generateEncoder(nh_, i, s_name, s_manufacturer);
			e->init();
			encoder_.push_back(e);
			RCLCPP_INFO(rclcpp::get_logger("Driver"), "[HardwareInterface] find encoder at index[%d]", i);
		}
		ROS_INFO_STREAM_NAMED("hardware_interface", s_name<<" manufacturer code: "<<s_manufacturer);

		
	}

	driverStatus.resize(servo_.size());

	velocity_interface_running_ = false;
	position_interface_running_ = false;

	srv_motor_enable_ = nh_.advertiseService("/motorEnable", &HardwareInterface::motor_enable_callback, this);
    set_entry_cln_    = nh_.serviceClient<agv_srvs::srv::SetCanopenEntry>("/set_canopen_entry");

}


void HardwareInterface::read() 
{
	for (int i=0; i<servo_.size(); i++) {
		joint_position_[servo_[i]->joint_index] = servo_[i]->Motor_GetCurrentPos();
		joint_velocity_[servo_[i]->joint_index] = servo_[i]->Motor_GetCurrentVel();
		joint_timestamp_[servo_[i]->joint_index] = servo_[i]->Motor_GetTimestamp();
		joint_effort_[servo_[i]->joint_index] = servo_[i]->Motor_GetCurrent();

		uint16_t errcode = servo_[i]->Motor_GetErrCode();
		if (errcode)
		{	
			agv_msgs::msg::AGVEvent event;
			event.ecode = agv_msgs::msg::AGVEvent::CANOPEN_SERVO_ERROR_EVENT;
			event.type = agv_msgs::msg::AGVEvent::ALARM;
			event.action = agv_msgs::msg::AGVEvent::ESTOP;
			event.ackReq = true;
			event.param1 = servo_[i]->joint_index;
			event.param2 = errcode;
			event.description = servo_[i]->joint_name + ":" + servo_[i]->Motor_Err2userErr(errcode);
			ep_.add(event, 1, event.param1);
		}
		else
		{
			ep_.clear(1, servo_[i]->joint_index);
		}


		driverStatus.at(i).jointName  = servo_[i]->joint_name;
		driverStatus.at(i).statusWord = servo_[i]->Motor_GetStatusWord();
		driverStatus.at(i).ready = servo_[i]->Motor_GetState() == MotorState::Ready;
		switch (servo_[i]->Motor_GetState()) {
			case MotorState::Off:
				driverStatus.at(i).motorState = "Off";
				break;
			case MotorState::Init:
				driverStatus.at(i).motorState = "Init";
				break;
			case MotorState::Shutdown:
				driverStatus.at(i).motorState = "Shutdown";
				break;
			case MotorState::Error:
				driverStatus.at(i).motorState = "Error";
				break;
			case MotorState::Halt:
				driverStatus.at(i).motorState = "Halt";
				break;
			case MotorState::Recover:
				driverStatus.at(i).motorState = "Recover";
				break;
			case MotorState::Ready:
				driverStatus.at(i).motorState = "Ready";
				break;
		}
	}

	driversInfo_.drivers = driverStatus;
	driversInfo_pub_.publish(driversInfo_);

	for (int i=0; i<encoder_.size(); i++) {
		joint_position_[encoder_[i]->joint_index_] = encoder_[i]->getRadValue();
		joint_velocity_[encoder_[i]->joint_index_] = 0.0;
		joint_timestamp_[encoder_[i]->joint_index_] = encoder_[i]->timestamp_;
		// RCLCPP_INFO(rclcpp::get_logger("Driver"), "[HardwareInterface] encoder[%d] index[%d] value[%.6f]", i, encoder_[i]->joint_index_, encoder_[i]->getRadValue());
	}

	// if (servo_.size() == 4) {
	// 	ROS_INFO_THROTTLE(1.0, "READ  servo[0]: %s  joint_index: %d  pos: %.6f  vel: %.6f", servo_[0]->joint_name.c_str(), servo_[0]->joint_index, joint_position_[servo_[0]->joint_index], joint_velocity_[servo_[0]->joint_index]);
	// 	ROS_INFO_THROTTLE(1.0, "READ  servo[1]: %s  joint_index: %d  pos: %.6f  vel: %.6f", servo_[1]->joint_name.c_str(), servo_[1]->joint_index, joint_position_[servo_[1]->joint_index], joint_velocity_[servo_[1]->joint_index]);
	// 	ROS_INFO_THROTTLE(1.0, "READ  servo[2]: %s  joint_index: %d  pos: %.6f  vel: %.6f", servo_[2]->joint_name.c_str(), servo_[2]->joint_index, joint_position_[servo_[2]->joint_index], joint_velocity_[servo_[2]->joint_index]);
	// 	ROS_INFO_THROTTLE(1.0, "READ  servo[3]: %s  joint_index: %d  pos: %.6f  vel: %.6f", servo_[3]->joint_name.c_str(), servo_[3]->joint_index, joint_position_[servo_[3]->joint_index], joint_velocity_[servo_[3]->joint_index]);
	// }


}

void HardwareInterface::write()
{
	for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) {
		for (unsigned int j = 0; j < servo_.size(); j++) {
			if (servo_[j]->joint_index == i) {
				if (servo_[j]->Motor_GetState() == Ready) {
					servo_[j]->Motor_SetTargetVelRad(joint_velocity_command_[i]);
				}
				else {
					joint_velocity_command_[i] = 0.0;
					servo_[j]->Motor_SetTargetVelRad(0.0);
				}
			}
		}
	}

	for (unsigned int i = 0; i < joint_position_command_.size(); i++) {
		for (unsigned int j = 0; j < servo_.size(); j++) {
			if (servo_[j]->joint_index == i) {
				if (servo_[j]->Motor_GetState() == Ready) {
					servo_[j]->Motor_SetTargetPosRad(joint_position_command_[i]);	
				}
			}
		}
	}
}

double HardwareInterface::getJointTimestamp(std::string joint_name)
{
	for (const auto& s : servo_) {
		if (s->joint_name == joint_name) {
			return s->Motor_GetTimestamp();
		}
	}

	for (const auto& e : encoder_) {
		if (e->joint_name_ == joint_name) {
			return e->timestamp_;
		}
	}

	ROS_WARN_THROTTLE(1.0, "[HardwareInterface] cannot get joint[%s] timestamp.", joint_name.c_str());
	return 0.0;
}

double HardwareInterface::getJointOldestTimestamp()
{
	double timestamp = rclcpp::Time::now().toSec();
	for (const auto& s : servo_) {
		if (s->Motor_GetTimestamp() < timestamp) {
			timestamp = s->Motor_GetTimestamp();
		}
	}

	for (const auto& e : encoder_) {
		if (e->timestamp_ < timestamp) {
			timestamp = e->timestamp_;
		}
	}

	return timestamp;
}

bool HardwareInterface::servoInitSingle(std::string joint_name)
{
	RCLCPP_INFO(rclcpp::get_logger("Driver"), "[HardwareInterface] init servo[%s]", joint_name.c_str());
	for (const auto& s : servo_) {
		if (s->joint_name == joint_name) {
			if (!s->Motor_InitParam()) {
				RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[HardwareInterface] motor[%s] param init fail", s->joint_name.c_str());
				return false;
			}

			if (s->Motor_GetState() != Ready) {
				if (s->Motor_Init()) {
					RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[HardwareInterface] motor[%s] init fail", s->joint_name.c_str());
					return false;
				}
				else {
					RCLCPP_INFO(rclcpp::get_logger("Driver"), "[HardwareInterface] motor[%s] init succeed.", s->joint_name.c_str());
					return true;
				}	
			}
			else {
				RCLCPP_INFO(rclcpp::get_logger("Driver"), "[HardwareInterface] motor[%s] ready, does not need to init.", s->joint_name.c_str());
				return true;
			}
		}
	}

	RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[HardwareInterface] InitSingle: motor[%s] cannot find.", joint_name);
	return false;
}

bool HardwareInterface::servoHaltSingle(std::string joint_name)
{
	RCLCPP_INFO(rclcpp::get_logger("Driver"), "[HardwareInterface] halt servo[%s]", joint_name.c_str());
	for (const auto& s :servo_) {
		if (s->joint_name == joint_name) {
			return s->Motor_Halt();
		}
	}

	RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[HardwareInterface] HaltSingle motor[%s] cannot find.", joint_name);
	return false;
}

bool HardwareInterface::servoRecoverSingle(std::string joint_name)
{
	RCLCPP_INFO(rclcpp::get_logger("Driver"), "[HardwareInterface] recover servo[%s]", joint_name.c_str());
	for (const auto& s :servo_) {
		if (s->joint_name == joint_name) {
			uint16_t result = 0;
			result |= s->Motor_GetErrCode();
			result |= (s->Motor_GetState() != MotorState::Ready);

			if (!(result == 0)) {
				return s->Motor_Recover();
			}
			else {
				RCLCPP_INFO(rclcpp::get_logger("Driver"), "[HardwareInterface] servo[%s] does not need to recover", joint_name.c_str());
				return true;
			}
		}
	}
	RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[HardwareInterface] RecoverSingle motor[%s] cannot find.", joint_name);
	return false;
}

bool HardwareInterface::servoInit()
{
	RCLCPP_INFO(rclcpp::get_logger("Driver"), "[HardwareInterface] servo init  servo size: %d", (int)servo_.size());
	ROS_ASSERT(servo_.size() > 0);

	std::vector<std::future<int>> futures;
	futures.resize(0);

	int result = 0;
	bool finished = true;

	for (int i = 0; i < servo_.size(); i++) {
		if (servo_[i] == nullptr) {
			RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[HardwareInterface] bad servo[%d] pointer. ", i);
			continue;
		}

		if (!servo_[i]->Motor_InitParam()) {
			RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[HardwareInterface] motor[%s] param init fail", servo_[i]->joint_name.c_str());
			agv_msgs::msg::AGVEvent event;
			event.ecode = agv_msgs::msg::AGVEvent::CANOPEN_SERVO_ERROR_EVENT;
			event.type = agv_msgs::msg::AGVEvent::ALARM;
			event.action = agv_msgs::msg::AGVEvent::ESTOP;
			event.ackReq = true;
			event.param1 = servo_[i]->joint_index;
			event.param2 = 0;
			event.description = "[HardwareInterface] servo init param failed.";
			ep_.add(event, 1, event.param1);
			ROS_INFO_STREAM("[HardwareInterface] Add new event:" << event);
			return false;
		}
		else {
			ep_.clear(1, servo_[i]->joint_index);
		}
	}
			

	for (int i = 0; i < servo_.size(); i++) {
		if (servo_[i] == nullptr) {
			RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[HardwareInterface] bad servo[%d] pointer. ", i);
			continue;
		}
		
		if (servo_[i]->servo_type == JACKING && servo_[i]->Motor_GetState() != Ready) {
			std::future<int> fut = std::async(std::launch::async, std::bind(&Motor::Motor_Init, servo_[i]));
			futures.emplace_back(std::move(fut));
			ROS_INFO_STREAM("[HardwareInterface] futures add jacking servo: "  << i << " " << servo_[i]->joint_name);	
		}
	}

	for (int i=0; i< futures.size(); i++) {
		try {
			result = futures[i].get();
		}
		catch (const std::future_error& e) {
			ROS_ERROR_STREAM("[HardwareInterface] future " << i << " get result error: " << e.what());
			finished = false;
		}

		if (result < 0) {
			ROS_ERROR_STREAM_NAMED("hardware_interface", "jacking servo init error! " << i);
			finished = false;
		}
		else {
			ROS_INFO_STREAM_NAMED("hardware_interface", "jacking servo init succeed. " << i);
		}
	}

	if (!finished) {
		RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[HardwareInterface] servo init return false. ");
		agv_msgs::msg::AGVEvent event;
		event.ecode = agv_msgs::msg::AGVEvent::CANOPEN_SERVO_ERROR_EVENT;
		event.type = agv_msgs::msg::AGVEvent::ALARM;
		event.action = agv_msgs::msg::AGVEvent::ESTOP;
		event.ackReq = true;
		event.param1 = EVENT_PARAM_ID_INIT_ERROR;
		event.param2 = 0;
		event.description = "[HardwareInterface] servo jacking init failed.";
		ROS_INFO_STREAM("[HardwareInterface] Add new event:" << event);
		ep_.add(event, 1, event.param1);
		return false;
	}
	else {
		ep_.clear(1, EVENT_PARAM_ID_INIT_ERROR);
	}
	
	futures.resize(0);
	result = 0;
	finished = true;
	for (int i = 0; i < servo_.size(); i++) {
		if (servo_[i] == nullptr) {
			RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[HardwareInterface] bad servo[%d] pointer. ", i);
			continue;
		}
		
		RCLCPP_INFO(rclcpp::get_logger("Driver"), "[HardwareInterface] %s  type: %d  state: %d", servo_[i]->joint_name.c_str(), servo_[i]->servo_type, servo_[i]->Motor_GetState());

		if (servo_[i]->servo_type != JACKING && servo_[i]->Motor_GetState() != Ready) {
			std::future<int> fut = std::async(std::launch::async, std::bind(&Motor::Motor_Init, servo_[i]));
			futures.emplace_back(std::move(fut));
			ROS_INFO_STREAM("[HardwareInterface] futures add servo: "  << i << " " << servo_[i]->joint_name);	
		}
	}

	for (int i=0; i< futures.size(); i++) {
		try {
			result = futures[i].get();
		}
		catch (const std::future_error& e) {
			ROS_ERROR_STREAM("[HardwareInterface] future " << i << " get result error: " << e.what());
			finished = false;
		}

		if (result < 0) {
			ROS_ERROR_STREAM_NAMED("hardware_interface", "servo init error! " << i);
			finished = false;
		}
		else {
			ROS_INFO_STREAM_NAMED("hardware_interface", "servo init succeed. " << i);
		}
	}

	if (!finished) {
		RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[HardwareInterface] servo init return false.");
		agv_msgs::msg::AGVEvent event;
		event.ecode = agv_msgs::msg::AGVEvent::CANOPEN_SERVO_ERROR_EVENT;
		event.type = agv_msgs::msg::AGVEvent::ALARM;
		event.action = agv_msgs::msg::AGVEvent::ESTOP;
		event.ackReq = true;
		event.param1 = EVENT_PARAM_ID_INIT_ERROR;
		event.param2 = 0;
		event.description = "[HardwareInterface] servo init failed.";
		ROS_INFO_STREAM("Add new event:" << event);
		ep_.add(event, 1, event.param1);
		return false;
	}
	else {
		ep_.clear(1, EVENT_PARAM_ID_INIT_ERROR);
	}

	velocity_interface_running_ = true;
	position_interface_running_ = true;
	return true;
}

bool HardwareInterface::servoRecover()
{
	ROS_ASSERT(servo_.size() > 0);
	std::vector<std::future<bool>> futures;
	futures.resize(0);
	for (int i = 0; i < servo_.size(); i++) {
		if (servo_[i] == nullptr) {
			RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[HardwareInterface] bad servo[%d] pointer. ", i);
			continue;
		}

		std::future<bool> fut = std::async(std::launch::async, std::bind(&Motor::Motor_Recover, servo_[i]));
		futures.emplace_back(std::move(fut));
	}

	bool result = false;
	bool finished = true;
	for (int i=0; i< futures.size(); i++) {
		try {
			result = futures[i].get();
		}
		catch (const std::future_error& e) {
			ROS_ERROR_STREAM("[HardwareInterface] future " << i << " get result error: " << e.what());
			finished = false;
		}

		if (!result) {
			ROS_ERROR_STREAM_NAMED("hardware_interface", "servo recover error! " << i);
			finished = false;
		}
		else {
			ROS_INFO_STREAM_NAMED("hardware_interface", "servo recover succeed. " << i);
		}
	}

	if (!finished) {
		RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[HardwareInterface] servo recover return false. ");
		agv_msgs::msg::AGVEvent event;
		event.ecode = agv_msgs::msg::AGVEvent::CANOPEN_SERVO_ERROR_EVENT;
		event.type = agv_msgs::msg::AGVEvent::ALARM;
		event.action = agv_msgs::msg::AGVEvent::ESTOP;
		event.ackReq = true;
		event.param1 = EVENT_PARAM_ID_RECOVER_ERROR;
		event.param2 = 0;
		event.description = "[HardwareInterface] servo recover failed.";
		ROS_INFO_STREAM("[HardwareInterface] Add new event:" << event);
		ep_.add(event, 1, event.param1);
		return false;
	}
	else {
		ep_.clear(1, EVENT_PARAM_ID_RECOVER_ERROR);
	}

	velocity_interface_running_ = true;
	position_interface_running_ = true;
	return true;
}


bool HardwareInterface::servoShutdown()
{
	return true;
}

bool HardwareInterface::servoHalt()
{
	ROS_ASSERT(servo_.size() > 0);
	velocity_interface_running_ = false;
	position_interface_running_ = false;

	std::vector<std::future<bool>> futures;
	futures.resize(0);
	for (int i = 0; i < servo_.size(); i++) {
		if (servo_[i] == nullptr) {
			RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[HardwareInterface] bad servo[%d] pointer. ", i);
			continue;
		}

		std::future<bool> fut = std::async(std::launch::async, std::bind(&Motor::Motor_Halt, servo_[i]));
		futures.emplace_back(std::move(fut));
	}

	bool result = false;
	bool finished = true;
	for (int i=0; i< futures.size(); i++) {
		try {
			result = futures[i].get();
		}
		catch (const std::future_error& e) {
			ROS_ERROR_STREAM("[HardwareInterface] future " << i << " get result error: " << e.what());
			finished = false;
		}

		if (!result) {
			ROS_ERROR_STREAM_NAMED("hardware_interface", "servo init error! " << i);
			finished = false;
		}
		else {
			ROS_INFO_STREAM_NAMED("hardware_interface", "servo init succeed. " << i);
		}
	}

	if (!finished) {
		RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[HardwareInterface] servo init return false. ");
		return false;
	}

	velocity_interface_running_ = false;
	position_interface_running_ = false;
	return true;
}



bool HardwareInterface::motor_enable_callback(agv_srvs::srv::MotorEnable::Request &req, agv_srvs::srv::MotorEnable::Response &resp)
{
	std::vector<std::future<bool>> futures;
	futures.resize(0);

	for (auto n : req.joint_names) {
		
		bool find = false;

		for (auto s : servo_) {
			if (n.compare(s->joint_name) == 0) {
				find = true;
				if (req.enable) {
					std::future<bool> fut = std::async(std::launch::async, std::bind(&Motor::Motor_Recover, s));
					futures.emplace_back(std::move(fut));
				}
				else {
					std::future<bool> fut = std::async(std::launch::async, std::bind(&Motor::Motor_Halt, s));
					futures.emplace_back(std::move(fut));	
				}
			}
		}

		if (!find) {
			RCLCPP_WARN(rclcpp::get_logger("Driver"), "[HardwareInterface] enable_srv: cannot find joint name: %s", n.c_str());
			resp.success = false;
			return resp.success;
		}
	}

	bool result = false;
	bool finished = true;
	for (int i=0; i< futures.size(); i++) {
		try {
			result = futures[i].get();
		}
		catch (const std::future_error& e) {
			ROS_ERROR_STREAM("[HardwareInterface] future " << i << " get result error: " << e.what());
			finished = false;
		}

		if (!result) {
			ROS_ERROR_STREAM_NAMED("hardware_interface", "servo enable/disable error! " << i);
			finished = false;
		}
		else {
			ROS_INFO_STREAM_NAMED("hardware_interface", "servo enable/disable succeed. " << i);
		}
	}

	if (!finished) {
		RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[HardwareInterface] servo enable/disable return false. ");
		resp.success = false;
	}
	
	resp.success = true;
	return resp.success;
}


bool HardwareInterface::hasError()
{
	uint16_t result = 0;
	for (int i=0; i<servo_.size(); i++) {
		result |= servo_[i]->Motor_GetErrCode();
		result |= (servo_[i]->Motor_GetState() != MotorState::Ready);
	}

	return !(result == 0);
}

bool HardwareInterface::callNmtService(uint32_t value)
{
	agv_srvs::srv::SetCanopenEntry set_entry;
    set_entry.request.bus_name = std::string("can0");
    set_entry.request.node_id = 0;
    set_entry.request.index = 0;
    set_entry.request.sub_index = 0;
    set_entry.request.value = value;

    if (set_entry_cln_.call(set_entry) && set_entry.response.success) {
        RCLCPP_INFO(rclcpp::get_logger("Driver"), "[ServoKeya] call canopen NMT service success.");
        return true;
    }
    else {
        RCLCPP_WARN(rclcpp::get_logger("Driver"), "[ServoKeya] call canopen NMT service fail.");
    	return false;
    }
    
    return false;
}


void HardwareInterface::publish_diagnostics(unsigned char level, std::string hardware_id, std::string message)
{
    diagnostic_msgs::msg::DiagnosticStatus status;
    status.level = level;
    status.name = "base_driver";
    status.hardware_id = hardware_id;
    status.message = message;
    diagnostics_.status.push_back(status);
    diagnostics_.header.stamp = rclcpp::Time::now();
    diagnostics_pub_.publish(diagnostics_);
    diagnostics_.status.resize(0);
}

/*
void HardwareInterface::eventTimerCb(const rclcpp::TimerEvent& event)
{
	// 2023.12.15 modify by haibo
	if (events_.size() > 0) {
		// Events have been published, nothing to do
	} else {
		// publish ordinary events
		rclcpp::Time timestamp = rclcpp::Time::now();
		agv_msgs::msg::AGVEvent event;
		eventHeader(event, timestamp);
		event.ecode = agv_msgs::msg::AGVEvent::NONE_EVENT;
		event.type = agv_msgs::msg::AGVEvent::INFO;
		event.action = agv_msgs::msg::AGVEvent::NONE;
		event.ackReq = true;
		event.param1 = 0;
		event.param2 = 0;
		event.eventTime = timestamp.toSec();
		event.description = "No event";
		event_pub_.publish(event);
	}

	for (int i = 0; i < servo_.size(); i++)
	{
		driverStatus.at(i).jointName  = servo_[i]->joint_name;
		driverStatus.at(i).statusWord = servo_[i]->Motor_GetStatusWord();
		driverStatus.at(i).ready = servo_[i]->Motor_GetState() == MotorState::Ready;
		switch (servo_[i]->Motor_GetState()) {
			case MotorState::Off:
				driverStatus.at(i).motorState = "Off";
				break;
			case MotorState::Init:
				driverStatus.at(i).motorState = "Init";
				break;
			case MotorState::Shutdown:
				driverStatus.at(i).motorState = "Shutdown";
				break;
			case MotorState::Error:
				driverStatus.at(i).motorState = "Error";
				break;
			case MotorState::Halt:
				driverStatus.at(i).motorState = "Halt";
				break;
			case MotorState::Recover:
				driverStatus.at(i).motorState = "Recover";
				break;
			case MotorState::Ready:
				driverStatus.at(i).motorState = "Ready";
				break;
		}
		
	}
	driversInfo_.drivers = driverStatus;
	driversInfo_pub_.publish(driversInfo_);
}
*/

} // namespace

